#include "utils.h"

Vector3D rotate(double angle, Vector3D vec, Vector3D axis, bool isDeg) {
   if (angle == 0.0 || (vec[0] == axis[0] && vec[1] == axis[1] & vec[2] == axis[2])) {
      return vec;
   } else {
      if (isDeg) {
         angle = angle * 3.1415 / 180.0;
      }
      axis = normalize(axis);
      double cos_a = cos(angle);
      double sin_a = sin(angle);
   
      Vector3D vp = vec * cos_a + cross_product(axis, vec) * sin_a + (axis * vec)
            * axis * (1.0 - cos_a);
      return vp;
   }
}

vector<string> readWords(string line) {
   string whitespace = " \n\t\r";

   if (line.size() == 0)
      return vector<string>();

   if (line[line.size() - 1] == '\\') {
      line[line.size() - 1] = ' ';
   }

   //split the line into words
   vector<string> words;
   string::size_type pos = 0;
   while (pos != string::npos) {
      pos = line.find_first_not_of(whitespace, pos);
      if (pos == string::npos)
         break;
      string::size_type eow = line.find_first_of(whitespace, pos);
      words.push_back(string(line, pos, eow - pos));
      pos = eow;
   }

   return words;
}

void loadBvhFail() {
   cout << "Error on file. Check the bvh format file specification" << endl;
}

Vector3D normalize(Vector3D &v) {
   Number l = sqrt(v*v);
   return v/l;
}

Number length(Vector3D &v) {
   return sqrt(v*v);
}
